copp\copp\copp2/
interpolation.rs

1//! Interpolation and profile-conversion utilities for second-order path parameterization.
2//!
3//! # Method identity
4//! This module serves both:
5//! - **Time-Optimal Path Parameterization (TOPP2)** workflows,
6//! - **Convex-Objective Path Parameterization (COPP2)** workflows.
7//!
8//! # Scope
9//! This module provides deterministic conversions among:
10//! - path-parameter profile `a(s) = \dot{s}^2`,
11//! - derivative-like profile `b(s) = \frac{1}{2}\frac{da}{ds}` on segments,
12//! - time mapping `t(s)` and inverse sampling `s(t)`.
13//!
14//! # Conventions
15//! - Path grid uses station samples `s[0..=n]`.
16//! - State profile `a` is node-based (`a.len() == s.len()`).
17//! - Profile `b` is segment-based (`b.len() == s.len() - 1`).
18//!
19//! # Example
20//! The example below converts a second-order profile from station samples to
21//! cumulative time and then samples the inverse map `s(t)`.
22//!
23//! ```rust
24//! # fn main() -> Result<(), copp::diag::CoppError> {
25//! use copp::InterpolationMode;
26//! use copp::solver::topp2_ra::{s_to_t_topp2, t_to_s_topp2};
27//!
28//! let s = [0.0, 0.5, 1.0];
29//! let a = [1.0, 1.0, 1.0];
30//!
31//! let (_t_final, t_s) = s_to_t_topp2(&s, &a, 0.0)?;
32//! let s_t = t_to_s_topp2(
33//!     &s,
34//!     &a,
35//!     &t_s,
36//!     InterpolationMode::UniformTimeGrid(0.0, 0.25, true),
37//! )?;
38//!
39//! assert_eq!(s_t.first().copied(), Some(0.0));
40//! assert_eq!(s_t.last().copied(), Some(1.0));
41//! # Ok(())
42//! # }
43//! ```
44
45use crate::copp::InterpolationMode;
46use crate::diag::{
47    CoppError, check_input_len_at_least, check_input_len_equal, check_input_not_empty,
48    check_input_not_nan_infinite, check_input_slice_non_negative,
49    check_input_slice_not_nan_infinite, check_input_strictly_increasing,
50};
51use itertools::izip;
52
53/// Compute segment profile `b` from node profile `a`.
54///
55/// # Definition
56/// For each segment `[s_k, s_{k+1}]`, this function computes:
57/// $b_k = \frac{1}{2}\frac{a_{k+1}-a_k}{s_{k+1}-s_k}$.
58///
59/// # Input contract
60/// - valid when `s.len() >= 2` and `a.len() == s.len()`;
61/// - `s` must be finite and strictly increasing;
62/// - `a` must contain only finite values.
63///
64/// # Returns
65/// Returns `b` with `b.len() == s.len() - 1`.
66///
67/// # Errors
68/// Returns [`CoppError::InvalidInput`](crate::diag::CoppError::InvalidInput) when dimensions, monotonicity, or numeric
69/// finiteness requirements are violated.
70///
71/// # Contract
72/// - Output ordering is consistent with segment ordering on `s.windows(2)`.
73/// - No allocation beyond returned vector and iterator temporaries.
74pub fn a_to_b_topp2(s: &[f64], a: &[f64]) -> Result<Vec<f64>, CoppError> {
75    check_topp2_sa("a_to_b_topp2", s, a)?;
76    Ok(s.windows(2)
77        .zip(a.windows(2))
78        .map(|(s_pair, a_pair)| 0.5 * (a_pair[1] - a_pair[0]) / (s_pair[1] - s_pair[0]))
79        .collect::<Vec<f64>>())
80}
81
82/// Compute cumulative time profile `t(s)` from `a(s)`.
83///
84/// # Semantics
85/// - `t_s[i]` is the time at station `s[i]`.
86/// - initial condition is `t_s[0] = t0`.
87/// - returns `(t_final, t_s)` where `t_final == *t_s.last().unwrap()`.
88///
89/// # Input contract
90/// - valid when `s.len() >= 2` and `a.len() == s.len()`;
91/// - `s`, `a`, and `t0` must contain only finite values;
92/// - `s` must be strictly increasing;
93/// - each interval must have finite positive speed denominator.
94///
95/// # Returns
96/// Returns `(t_final, t_s)` with `t_s.len() == s.len()` on valid input.
97///
98/// # Errors
99/// Returns [`CoppError::InvalidInput`](crate::diag::CoppError::InvalidInput) when dimensions, monotonicity, positivity,
100/// or numeric finiteness requirements are violated.
101///
102/// # Contract
103/// - `t_s` is monotonically increasing when `a` is nonnegative and `s` is increasing.
104/// - `t_s[0] == t0` always holds on valid input.
105pub fn s_to_t_topp2(s: &[f64], a: &[f64], t0: f64) -> Result<(f64, Vec<f64>), CoppError> {
106    check_topp2_sa("s_to_t_topp2", s, a)?;
107    check_input_not_nan_infinite("s_to_t_topp2", "t0", t0)?;
108    check_topp2_time_denominator("s_to_t_topp2", a)?;
109    // Map s to t
110    let mut t_s = Vec::<f64>::with_capacity(s.len()); // t_s[i] = t(s[i]), begin from t0
111    let mut t_prev = t0;
112    t_s.push(t_prev);
113    for (s_pair, a_pair) in s.windows(2).zip(a.windows(2)) {
114        t_prev += 2.0 * (s_pair[1] - s_pair[0]) / (a_pair[0].sqrt() + a_pair[1].sqrt());
115        t_s.push(t_prev);
116    }
117    if !t_prev.is_finite() || t_s.iter().any(|value| !value.is_finite()) {
118        return Err(CoppError::InvalidInput(
119            "s_to_t_topp2".into(),
120            "computed time profile contains NaN or infinity".into(),
121        ));
122    }
123    Ok((t_prev, t_s))
124}
125
126/// Interpolate inverse mapping `s(t)` from `a(s)` and sampled `t(s)`.
127///
128/// # Modes
129/// - [`UniformTimeGrid`](crate::InterpolationMode::UniformTimeGrid)`(t0, dt, include_final)`: generate uniform time samples;
130/// - `NonUniformTimeGrid(t_sample)`: use caller-provided increasing samples.
131///
132/// # Input contract
133/// - requires `s.len() >= 2`, `a.len() == s.len()`, `t_s.len() == s.len()`;
134/// - requires `t_s` strictly increasing.
135/// - all profile and time-grid values must be finite.
136///
137/// # Output semantics
138/// - output length matches requested sample count in each mode;
139/// - for out-of-range time samples, output entries are `NaN`.
140///
141/// # Returns
142/// Returns sampled `s(t)` values according to `mode`.
143///
144/// # Errors
145/// Returns [`CoppError::InvalidInput`](crate::diag::CoppError::InvalidInput) when dimensions, monotonicity, positivity,
146/// or numeric finiteness requirements are violated.
147///
148/// # Contract
149/// - preserves requested sample order;
150/// - malformed input is reported as [`CoppError::InvalidInput`](crate::diag::CoppError::InvalidInput).
151pub fn t_to_s_topp2(
152    s: &[f64],
153    a: &[f64],
154    t_s: &[f64],
155    mode: InterpolationMode<'_>,
156) -> Result<Vec<f64>, CoppError> {
157    check_topp2_sa("t_to_s_topp2", s, a)?;
158    check_topp2_time_denominator("t_to_s_topp2", a)?;
159    check_input_len_equal(
160        "t_to_s_topp2",
161        "`t_s.len()`",
162        t_s.len(),
163        "`s.len()`",
164        s.len(),
165    )?;
166    check_input_slice_not_nan_infinite("t_to_s_topp2", "t_s", t_s)?;
167    check_input_strictly_increasing("t_to_s_topp2", "t_s", t_s)?;
168    match mode {
169        InterpolationMode::UniformTimeGrid(t0, dt, include_final) => {
170            check_input_not_nan_infinite("t_to_s_topp2", "t0", t0)?;
171            check_input_not_nan_infinite("t_to_s_topp2", "dt", dt)?;
172            if dt <= 0.0 {
173                return Err(CoppError::InvalidInput(
174                    "t_to_s_topp2".into(),
175                    format!("`dt` = {dt} must be positive"),
176                ));
177            }
178            // num_t * dt + t0 <= t_final
179            let num_t = ((t_s.last().unwrap() - t0) / dt).floor() as usize;
180            let mut s_t =
181                t_to_s_topp2_core(s, a, t_s, (0..num_t).map(|i| t0 + i as f64 * dt), num_t);
182            if include_final {
183                let flag = if s_t.is_empty() {
184                    t0 <= *t_s.last().unwrap()
185                } else {
186                    *s_t.last().unwrap() < *s.last().unwrap()
187                };
188                if flag {
189                    s_t.push(*s.last().unwrap());
190                }
191            }
192            Ok(s_t)
193        }
194        InterpolationMode::NonUniformTimeGrid(t_sample) => {
195            check_input_not_empty("t_to_s_topp2", "`t_sample`", t_sample.len())?;
196            check_input_slice_not_nan_infinite("t_to_s_topp2", "t_sample", t_sample)?;
197            check_input_strictly_increasing("t_to_s_topp2", "t_sample", t_sample)?;
198            Ok(t_to_s_topp2_core(
199                s,
200                a,
201                t_s,
202                t_sample.iter().cloned(),
203                t_sample.len(),
204            ))
205        }
206    }
207}
208
209/// Check the shared TOPP2 profile shape and station-ordering contract.
210///
211/// The second-order interpolation routines all require a node-based `a(s)`
212/// profile sampled on the same strictly increasing station grid `s`.
213fn check_topp2_sa(function_name: &str, s: &[f64], a: &[f64]) -> Result<(), CoppError> {
214    check_input_len_at_least(function_name, "`s.len()`", s.len(), 2)?;
215    check_input_len_equal(function_name, "`a.len()`", a.len(), "`s.len()`", s.len())?;
216    check_input_slice_not_nan_infinite(function_name, "s", s)?;
217    check_input_slice_not_nan_infinite(function_name, "a", a)?;
218    check_input_strictly_increasing(function_name, "s", s)
219}
220
221/// Check the TOPP2 time-integration denominator.
222///
223/// The mapping from `s` to `t` divides by
224/// `sqrt(a[i]) + sqrt(a[i + 1])`; this helper rejects negative `a` values and
225/// zero-speed intervals before the integration loop.
226fn check_topp2_time_denominator(function_name: &str, a: &[f64]) -> Result<(), CoppError> {
227    check_input_slice_non_negative(function_name, "a", a)?;
228    if let Some((index, _pair)) = a.windows(2).enumerate().find(|(_, pair)| {
229        let denominator = pair[0].sqrt() + pair[1].sqrt();
230        !denominator.is_finite() || denominator <= 0.0
231    }) {
232        return Err(CoppError::InvalidInput(
233            function_name.into(),
234            format!(
235                "`sqrt(a[{index}]) + sqrt(a[{}])` must be finite and positive",
236                index + 1
237            ),
238        ));
239    }
240    Ok(())
241}
242
243/// Core inverse interpolation kernel for [`t_to_s_topp2`](crate::solver::topp2_ra::t_to_s_topp2).
244///
245/// It consumes increasing `t_sample` values and emits corresponding `s(t)` by
246/// segment-wise inversion with quadratic-in-`a` local model.
247fn t_to_s_topp2_core(
248    s: &[f64],
249    a: &[f64],
250    t_s: &[f64],
251    mut t_sample: impl Iterator<Item = f64>,
252    len_t_sample: usize,
253) -> Vec<f64> {
254    let &t_start = t_s.first().unwrap();
255    // Map t to s
256    let mut s_t = Vec::<f64>::with_capacity(len_t_sample + 1); // s_t[i] = s(t[i])
257    let Some(mut t_curr) = t_sample.next() else {
258        return vec![];
259    };
260    while t_curr < t_start {
261        s_t.push(f64::NAN);
262        let Some(t) = t_sample.next() else {
263            return s_t;
264        };
265        t_curr = t;
266    }
267
268    for (s_pair, a_pair, t_pair) in izip!(s.windows(2), a.windows(2), t_s.windows(2)) {
269        while t_curr <= t_pair[1] {
270            s_t.push(
271                s_pair[0]
272                    + inverse_2order(
273                        a_pair[0],
274                        (a_pair[1] - a_pair[0]) / (s_pair[1] - s_pair[0]),
275                        0.0,
276                        t_curr - t_pair[0],
277                    ),
278            );
279            let Some(t) = t_sample.next() else {
280                return s_t;
281            };
282            t_curr = t;
283        }
284    }
285
286    s_t.push(f64::NAN);
287    while t_sample.next().is_some() {
288        s_t.push(f64::NAN);
289    }
290    s_t
291}
292
293/// Solve `x_right` from the integral equation
294/// $dt = \int_{x_{left}}^{x_{right}} \frac{dx}{\sqrt{c_0 + c_1 x}}$.
295#[inline]
296fn inverse_2order(c0: f64, c1: f64, x_left: f64, dt: f64) -> f64 {
297    if dt == 0.0 {
298        x_left
299    } else if c1.abs() > f64::EPSILON {
300        (((c0 + c1 * x_left).sqrt() + 0.5 * c1 * dt).powi(2) - c0) / c1
301    } else if c0.abs() > f64::EPSILON {
302        x_left + c0.sqrt() * dt
303    } else {
304        f64::INFINITY
305    }
306}
copp\copp\copp2/interpolation.rs

copp\copp\copp2/
interpolation.rs
